from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os

def generate_launch_description():
    # Get the path to playgrounds directory
    playground_path = "/home/yz/Code/EPSILON/simulator/playgrounds/highway_v1.0"
    
    # Define file paths
    vehicle_info_path = os.path.join(playground_path, 'vehicle_set.json')
    map_path = os.path.join(playground_path, 'obstacles_norm.json')
    lane_net_path = os.path.join(playground_path, 'lane_net_norm.json')
    
    # Declare launch arguments
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='true',
        description='Use simulation time'
    )
    
    vehicle_info_path_arg = DeclareLaunchArgument(
        'vehicle_info_path',
        default_value=vehicle_info_path,
        description='Path to vehicle info JSON file'
    )
    
    map_path_arg = DeclareLaunchArgument(
        'map_path',
        default_value=map_path,
        description='Path to map JSON file'
    )
    
    lane_net_path_arg = DeclareLaunchArgument(
        'lane_net_path',
        default_value=lane_net_path,
        description='Path to lane network JSON file'
    )

    # Simulator node with all required parameters (using static paths for now)
    phy_simulator_node = Node(
        package='phy_simulator',
        executable='phy_simulator_planning_node',
        name='phy_simulator',
        parameters=[
            {
                'use_sim_time': True,
                'vehicle_info_path': vehicle_info_path,
                'map_path': map_path,
                'lane_net_path': lane_net_path
            }
        ],
        remappings=[
            ('~/output/arena_info_static', '/map/arena_info_static'),
            ('~/output/arena_info_dynamic', '/perception/arena_info_dynamic'),
            ('~/input/control_signal', '/control/control_signal')
        ],
        output='screen'
    )
    
    # Note: Vehicle model node is commented out as the executable doesn't exist yet
    # Uncomment when vehicle_model package provides a proper executable
    # vehicle_model_node = Node(
    #     package='vehicle_model',
    #     executable='vehicle_model_node',
    #     name='vehicle_model',
    #     parameters=[
    #         {'use_sim_time': LaunchConfiguration('use_sim_time')}
    #     ],
    #     remappings=[
    #         ('~/input/trajectory', '/planning/trajectory'),
    #         ('~/output/control_signal', '/control/control_signal'),
    #         ('~/output/vehicle_state', '/vehicle/state')
    #     ],
    #     output='screen'
    # )

    return LaunchDescription([
        use_sim_time_arg,
        vehicle_info_path_arg,
        map_path_arg,
        lane_net_path_arg,
        phy_simulator_node,
        # vehicle_model_node,  # Commented out until available
    ])